/**
 * @file surgewheel_task.c/h
 * @author sunny quan 
 * @brief 摩擦轮控制任务线程
 * @version 0.2
 * @date 2023-03-25
 *
 * @copyright Copyright (c) 2022 SPR
 *
 */
#include "surgewheel_task.h"
#include "AHRS_MiddleWare.h"
#include "cmsis_os.h"
#include "main.h"
#include "arm_math.h"
#include "chassis_behaviour.h"
#include "detect_task.h"
#include "INS_task.h"
#include "odom_serial_task.h"
#include "board_talk.h"

#define rc_deadband_limit(input, output, dealine)    \
  {                                                  \
    if ((input) > (dealine) || (input) < -(dealine)) \
    {                                                \
      (output) = (input);                            \
    }                                                \
    else                                             \
    {                                                \
      (output) = 0;                                  \
    }                                                \
  }

  /* flag */
extern uint8_t load_motor_init_flag ;		//上弹电机初始化flag
extern uint8_t up_limit_flag;				//上限位开关flag，该flag为1时立即停止上弹电机和摩擦轮
  
//摩擦轮运动数据
chassis_move_t surgewheel;

	float gpio_check=0;
	int16_t steer_init_flag;
	fp32 cha_now_ang=0,cha_tar_ang=0;
	fp32 cha_now_vel=0,cha_tar_vel=0;
	
/**
 * @brief          底盘任务，间隔 CHASSIS_CONTROL_TIME_MS 2ms
 * @param[in]      pvParameters: 空
 * @retval         none
 */
extern void surgewheel_task(void const *pvParameters)
{
	//空闲一段时间
	vTaskDelay(SURGEWHEEL_TASK_INIT_TIME);
	//摩擦轮初始化	
	surgewheel_init(&surgewheel);

  while (1)
  {
	
    //根据遥控器设置底盘控制模式,全部默认设置CHASSIS_VECTOR_RAW模式
		chassis_set_mode(&surgewheel);
		
    //底盘数据更新,更新摩擦轮速度 加速度
		chassis_feedback_update(&surgewheel);
		
    //底盘控制量设置,遥控器/上层/debug模式，获取不同的目标值
		surgewheel_set_contorl(&surgewheel);
	  
    //底盘控制PID计算
		surgewheel_control_loop(&surgewheel);
		
	
	
	//未完成上弹电机初始化时不允许摩擦轮旋转
	if( !load_motor_init_flag )
	{
		surgewheel_stay_cmd( &surgewheel );
	}
	
	CAN_cmd_surgewheel(	surgewheel.motor_chassis[0].give_current,	surgewheel.motor_chassis[1].give_current,
						surgewheel.motor_chassis[2].give_current,	surgewheel.motor_chassis[3].give_current);
	
	

    //系统延时
    vTaskDelay(SURGEWHEEL_CONTROL_TIME_MS);
	}
}

/**
* @brief           摩擦轮速度控制为0，判断aiming_flag为假时中使用
 * @param[in]      chassis_move_t型结构体，具体来说是task中给的surgewheel
 * @retval         在->motor_speed_pid[i].out中体现，通过task把out给到电调
 */
static void surgewheel_stay_cmd(chassis_move_t *chassis_move_stay)
{
	for(int i=0;i<4;++i)
	{
		PID_calc(&chassis_move_stay->motor_speed_pid[i], chassis_move_stay->motor_chassis[i].speed, 0);
		chassis_move_stay->motor_chassis[i].give_current = chassis_move_stay->motor_speed_pid[i].out;
	}
}
		
/**
 * @brief          初始化"chassis_move"变量，包括pid初始化， 遥控器指针初始化，3508底盘电机指针初始化，云台电机初始化，陀螺仪角度指针初始化
 * @param[out]     chassis_move_init:"chassis_move"变量指针.
 * @retval         none
 */
static void surgewheel_init(chassis_move_t *chassis_move_init)
{
  if (chassis_move_init == NULL)
  {
    return;
  }

   const static float steer_x_order_filter[1] = {CHASSIS_ACCEL_X_NUM*2};
   const static float steer_y_order_filter[1] = {CHASSIS_ACCEL_Y_NUM};
   
   const static float driver_x_order_filter[1] = {CHASSIS_ACCEL_X_NUM};
   const static float driver_y_order_filter[1] = {CHASSIS_ACCEL_Y_NUM};
  uint8_t i;

  //底盘开机状态为原始
  chassis_move_init->chassis_mode = CHASSIS_DEBUG_RAW;
  //获取遥控器指针
  chassis_move_init->chassis_RC = get_remote_control_point();
  //获取陀螺仪姿态角指针
  chassis_move_init->chassis_INS_angle = get_INS_angle_point();

  //获取底盘电机数据指针，初始化驱动轮及方向轮PID
   first_order_filter_init(&chassis_move_init->chassis_cmd_slow_set_vx, CHASSIS_CONTROL_TIME, driver_x_order_filter);
   first_order_filter_init(&chassis_move_init->chassis_cmd_slow_set_vy, CHASSIS_CONTROL_TIME, driver_y_order_filter);
	for(int i=0;i<4;++i)
	{
		chassis_move_init->motor_chassis[i].chassis_motor_measure = get_chassis_motor_measure_point(i);
		PID_init(&chassis_move_init->motor_speed_pid[i], M3508_DRIVER_SPEED_PID_KP, M3508_DRIVER_SPEED_PID_KI, M3508_DRIVER_SPEED_PID_KD, M3508_DRIVER_SPEED_PID_MAX_OUT, M3508_DRIVER_SPEED_PID_MAX_IOUT);
	}
}

/**
 * @brief          设置底盘控制模式，主要在'chassis_behaviour_mode_set'函数中改变
 * @param[out]     chassis_move_mode:"chassis_move"变量指针.
 * @retval         none
 */
static void chassis_set_mode(chassis_move_t *chassis_move_mode)
{
  if (chassis_move_mode == NULL)
  {
	return;
  }
  
	chassis_behaviour_mode_set(chassis_move_mode);
  
}

/**
 * @brief          底盘测量数据更新，包括电机速度，欧拉角度，机器人速度
 * @param[out]     chassis_move_update:"chassis_move"变量指针.
 * @retval         none
 */	
/**
 * @brief          底盘测量数据更新，包括电机速度，欧拉角度，机器人速度
 * @retval         none
 */
static void chassis_feedback_update( chassis_move_t *chassis_move )
{
  uint8_t i = 0;
  for (i = 0; i < 4; i++)
  {
    //更新电机速度，加速度是速度的PID微分
    chassis_move->motor_chassis[i].speed = CHASSIS_MOTOR_RPM_TO_VECTOR_SEN * chassis_move->motor_chassis[i].chassis_motor_measure->speed_rpm;
    chassis_move->motor_chassis[i].accel = chassis_move->motor_speed_pid[i].Dbuf[0] * CHASSIS_CONTROL_FREQUENCE;
  }

//  //更新底盘纵向速度 x，平移速度y，旋转速度wz，坐标系为右手系
//  chassis_move.vx = (chassis_move.motor_chassis[0].speed - chassis_move.motor_chassis[1].speed - chassis_move.motor_chassis[2].speed + chassis_move.motor_chassis[3].speed) * MOTOR_SPEED_TO_CHASSIS_SPEED_VX;
//  chassis_move.vy = (chassis_move.motor_chassis[0].speed + chassis_move.motor_chassis[1].speed - chassis_move.motor_chassis[2].speed - chassis_move.motor_chassis[3].speed) * MOTOR_SPEED_TO_CHASSIS_SPEED_VY;
//  chassis_move.wz = (-chassis_move.motor_chassis[0].speed - chassis_move.motor_chassis[1].speed - chassis_move.motor_chassis[2].speed - chassis_move.motor_chassis[3].speed) * MOTOR_SPEED_TO_CHASSIS_SPEED_WZ / MOTOR_DISTANCE_TO_CENTER;

//  //计算底盘姿态角度
//  chassis_move.chassis_yaw = rad_format(INS_data.angle_yaw - gimbal_control.gimbal_yaw_motor.relative_angle);
//  chassis_move.chassis_pitch = rad_format(INS_data.angle_pitch - gimbal_control.gimbal_pitch_motor.relative_angle);
//  chassis_move.chassis_roll = INS_data.angle_roll;
}

/**
 * @brief          设置摩擦轮速度设置值
 * @param[out]     chassis_move_update:"chassis_move"变量指针.
 * @retval         none
 */
static void surgewheel_set_contorl(chassis_move_t *surgewheel_control)
{
	if( surgewheel_control->chassis_mode == SURGEWHEEL_STAY )
	{
		/* 该模式下保持电机静止 */
		for( int8_t temp = 0 ; temp<4 ; temp++ )
		{
			surgewheel_control->motor_chassis[temp].speed_set = 0;
		}
	
	}
	else if( surgewheel_control->chassis_mode == SURGEWHEEL_DEBUG ) 
	{
		
		/* DEBUG模式下暂时通过右摇杆上下控制速度，调PID等 */
		surgewheel_control->speed_set_all += (1.0 * surgewheel_control->chassis_RC->rc.ch[1] / 660 );
		
		surgewheel_control->motor_chassis[0].speed_set = -1.0*surgewheel_control->speed_set_all;
		surgewheel_control->motor_chassis[1].speed_set = -1.0*surgewheel_control->speed_set_all;
		surgewheel_control->motor_chassis[2].speed_set = surgewheel_control->speed_set_all;
		surgewheel_control->motor_chassis[3].speed_set = surgewheel_control->speed_set_all;
		
	}
	
	
}

static void surgewheel_control_loop(chassis_move_t *surgewheel_control)
{
	for( int8_t temp = 0 ; temp<4 ; temp++ )
	{	
		//速度环控制
		surgewheel_control->motor_chassis[temp].give_current = PID_calc( &(surgewheel_control->motor_speed_pid[temp]), surgewheel_control->motor_chassis[temp].chassis_motor_measure->speed_rpm, surgewheel_control->motor_chassis[temp].speed_set);

	}
	
	
}



